/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package search;

import java.util.ArrayList;
import java.util.Comparator;
import java.util.List;
import java.util.PriorityQueue;
import java.util.Queue;
import java.util.Random;
import plot.GNUPlot;

/**
 *
 * @author dheath
 */
public class AStarSearch implements Search 
{
    private int nodesChecked;
     public PriorityNode searchWorld(WorldNode start, WorldNode end, GNUPlot plot)
    {
         
         nodesChecked = 0;
         
        Queue<PriorityNode> queue = new PriorityQueue(11, new PriorityNodeComparator());

        List<VisitedNode> visited = new ArrayList();
        queue.add(new PriorityNode(start, heuristic(start,end), null,0));
        PriorityNode endPath = null;

        double diag = Math.sqrt(start.getLength() * start.getLength() + start.getLength() * start.getLength()) - start.getLength();
      // System.out.println(diagCost+" diagCost");
       
        while (!queue.isEmpty())
        {
            PriorityNode pcurrent = queue.remove();

            WorldNode current = pcurrent.getNode();

            if (current == end)
            {
                endPath = pcurrent;
                break;
            }
            
            boolean processNode = true;
            int i = visited.indexOf(new VisitedNode(current, pcurrent.getCostSoFar()));
            if(i != -1)
            {
                if(visited.get(i).getTotalCost() <= pcurrent.getCostSoFar())
                {
                    processNode = false;
                }
                else
                {
                    visited.remove(new VisitedNode(current, pcurrent.getCostSoFar()));
                }
            }
            if (processNode)
            {
                nodesChecked++;
                //costSoFar += pcurrent.getTotalCost();
                //plot.printNode(current.getCenter(), 1);
               //plot.printX(current.getCenter(), 1);

                visited.add(new VisitedNode(current,pcurrent.getCostSoFar()));


                if (current.getWest() != null && current.getWest().getCost() < Double.POSITIVE_INFINITY)
                {
                    queue.add(new PriorityNode(current.getWest(),  heuristic(current.getWest(),end), pcurrent,0 + pcurrent.getCostSoFar()));
                }
                if (current.getSouthWest() != null && current.getSouthWest().getCost() < Double.POSITIVE_INFINITY)
                {
                    queue.add(new PriorityNode(current.getSouthWest(),  heuristic(current.getSouthWest(),end), pcurrent, diag + pcurrent.getCostSoFar()));
                }
                if (current.getSouth() != null && current.getSouth().getCost() < Double.POSITIVE_INFINITY)
                {
                    queue.add(new PriorityNode(current.getSouth(),  heuristic(current.getSouth(),end), pcurrent, 0 + pcurrent.getCostSoFar()));
                }
                if (current.getSouthEast() != null && current.getSouthEast().getCost() < Double.POSITIVE_INFINITY)
                {
                    queue.add(new PriorityNode(current.getSouthEast(), heuristic(current.getSouthEast(),end), pcurrent, diag + pcurrent.getCostSoFar()));
                }
                if (current.getEast() != null && current.getEast().getCost() < Double.POSITIVE_INFINITY)
                {
                    queue.add(new PriorityNode(current.getEast(),  heuristic(current.getEast(),end), pcurrent, 0 + pcurrent.getCostSoFar()));
                }
                if (current.getNorthEast() != null && current.getNorthEast().getCost() < Double.POSITIVE_INFINITY)
                {
                    queue.add(new PriorityNode(current.getNorthEast(),  heuristic(current.getNorthEast(),end), pcurrent, diag + pcurrent.getCostSoFar()));
                }
                if (current.getNorth() != null && current.getNorth().getCost() < Double.POSITIVE_INFINITY)
                {
                    queue.add(new PriorityNode(current.getNorth(), heuristic(current.getNorth(),end), pcurrent, 0 + pcurrent.getCostSoFar()));
                }
                if (current.getNorthWest() != null && current.getNorthWest().getCost() < Double.POSITIVE_INFINITY)
                {
                    queue.add(new PriorityNode(current.getNorthWest(),  heuristic(current.getNorthWest(),end), pcurrent, 0 + pcurrent.getCostSoFar()));
                }
            }

        }

        //printPath(endPath, plot);
        return endPath;
    }
    
    private double heuristic(WorldNode here, WorldNode end)
    {
        //Random rand = new Random();
       // return rand.nextDouble()*600;
        
        //////MANHATTEN
        return Math.abs(here.getCenter().getX()-end.getCenter().getX()) + Math.abs(here.getCenter().getY()-end.getCenter().getY());
        
        ///STRAIGHT LINE
       
        
       /* return Math.sqrt( Math.pow(here.getCenter().getX() - end.getCenter().getX(),2) +
                Math.pow(here.getCenter().getY() - end.getCenter().getY(),2));//+factor;*/
    }
    
    private void printPath(PriorityNode end, GNUPlot plot)
    {
        
        double cost = end.getCostSoFar();
        int num = 0;
        WorldNode current = null;
        if (end != null)
        {
            current = end.getNode();
            end = end.getParent();
        }
        while (end != null)
        {
            num++;
            //cost += current.getCost();
            plot.printLine(current.getCenter(), end.getNode().getCenter(), 2, true);
            current = end.getNode();

            end = end.getParent();
        }
        System.out.println("TOTAL COST OF PATH: "+cost);
        System.out.println("Total Nodes in path: "+num);
        System.out.println("Total nodes expanded: "+nodesChecked);
    }
    
    class VisitedNode
    {
        private WorldNode node;
        private double totalCost;
        
        public VisitedNode(WorldNode node, double totalCost)
        {
            this.node = node;
            this.totalCost = totalCost;
        }

        public WorldNode getNode()
        {
            return node;
        }

        public void setNode(WorldNode node)
        {
            this.node = node;
        }

        public double getTotalCost()
        {
            return totalCost;
        }

        public void setTotalCost(double totalCost)
        {
            this.totalCost = totalCost;
        }
        
        public boolean equals(Object o)
        {
            VisitedNode v = (VisitedNode)o;
            
            return v.getNode() == this.getNode();
        }
    }
    

    class PriorityNodeComparator implements Comparator
    {

        public int compare(Object o1, Object o2)
        {
            PriorityNode pn1 = (PriorityNode) o1;
            PriorityNode pn2 = (PriorityNode) o2;

            return (int) (pn1.getTotalCost() - pn2.getTotalCost());
        }
    }
}
